#include "ToolMovementFCN.h"

void ToolMovementFCN::Init(const std::list<MATHLIB::vector3> &A, const std::list<MYFLOAT> &b)
{
	assert(A.size() == b.size());
	size_t k, w;
	m_H.resize(A.size());
	m_b.resize(b.size());
	size_t count = m_H.size();

	std::list<MYFLOAT>::const_iterator ib = b.begin();
	for(k=0; k<count; ++k) {
		m_H[k].resize(count, 0.0);
		m_b[k] = static_cast<double>(*ib);
		//std::cout<<"d: "<<*ib<<std::endl;
		++ib;
	}

	std::list<MATHLIB::vector3>::const_iterator ifirst, isec;
	ifirst = A.begin();
	k = w = 0;
	while(ifirst != A.end() && w < count) {
		isec = ifirst;
		++isec;
		k = w+1;
		m_H[w][w] = 0.5 * MATHLIB::DotProduct(*ifirst, *ifirst);
		while(isec != A.end() && k < count) {
			m_H[w][k] = m_H[k][w] = 0.5 * MATHLIB::DotProduct(*ifirst, *isec);
			++k;
		}
		//std::cout<<"n: "<<ifirst->x<<
//			", "<<ifirst->y<<", "<<ifirst->z
//			<<std::endl;
		++ifirst;
		++w;
	}
}

double ToolMovementFCN::operator ()(const std::vector<double> &a_lambda) const
{
	assert(a_lambda.size() == m_b.size());
	double dot, dot2, count;
	size_t w,k;

	dot2 = dot = 0.0;
	count = m_b.size();
	for(w=0; w<count; ++w) {
		for(k=0; k<count; ++k) {
			dot += a_lambda[w] * m_H[w][k] * a_lambda[k];
		}
		dot2 += m_b[w] * a_lambda[w];
	}
	//std::cout<<"f: "<<0.5 * dot - dot2<<"\t";
	return 0.5 * dot - dot2;
}

std::vector<double> ToolMovementFCN::Gradient(const std::vector<double> &a_lambda) const
{
	std::vector<double> res;
	res.resize(1, 0.0);
	return res;
}